#include "ps2.h"
/*********************************************************     
**********************************************************/	 

int PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY;

uint16 Handkey;	// 按键值读取，零时存储。
uint8 Comd[2]={0x01,0x42};	//开始命令。请求数据
uint8 Data[9]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; //数据存储数组
uint16 MASK[]={
    PSB_SELECT,
    PSB_L3,
    PSB_R3 ,
    PSB_START,
    PSB_PAD_UP,
    PSB_PAD_RIGHT,
    PSB_PAD_DOWN,
    PSB_PAD_LEFT,
    PSB_L2,
    PSB_R2,
    PSB_L1,
    PSB_R1 ,
    PSB_GREEN,
    PSB_RED,
    PSB_BLUE,
    PSB_PINK
	};	//按键值与按键明



//手柄接口初始化    输入  DI->PA0->B5 
//                  输出  DO->PA1->B4    CS->PA2->B3  CLK->PA3->A15
void PS2_Init(void)
{

	PS2_MSP_Init();
	PS2_SetInit();
	
}

//向手柄发送命令
void PS2_Cmd(uint8 CMD)
{
	volatile uint16 ref=0x01;
	Data[1] = 0;
	for(ref=0x01;ref<0x0100;ref<<=1)
	{
		if(ref&CMD)
		{
			DO_H;                   //输出一位控制位
		}
		else DO_L;

		CLK_H;                        //时钟拉高
		__DELAY_5US;
		CLK_L;
		__DELAY_5US;
		CLK_H;
		if(DI)
			Data[1] = ref|Data[1];
	}
	systick_delay_us(16);
}
//判断是否为红灯模式,0x41=模拟绿灯，0x73=模拟红灯
//返回值；0，红灯模式
//		  其他，其他模式
uint8 PS2_RedLight(void)
{
	CS_L;
	PS2_Cmd(Comd[0]);  //开始命令
	PS2_Cmd(Comd[1]);  //请求数据
	CS_H;
	if( Data[1] == 0X73)   return 0 ;
	else return 1;

}
//读取手柄数据
void PS2_ReadData(void)
{
	volatile uint8 byte=0;
	volatile uint16 ref=0x01;
	CS_L;
	PS2_Cmd(Comd[0]);  //开始命令
	PS2_Cmd(Comd[1]);  //请求数据
	for(byte=2;byte<9;byte++)          //开始接受数据
	{
		for(ref=0x01;ref<0x100;ref<<=1)
		{
			CLK_H;
			__DELAY_5US;
			CLK_L;
			__DELAY_5US;
			CLK_H;
		      if(DI)
		      Data[byte] = ref|Data[byte];
		}
		systick_delay_us(16);
	}
	CS_H;
}

//对读出来的PS2的数据进行处理,只处理按键部分  
//只有一个按键按下时按下为0， 未按下为1
uint8 PS2_DataKey()
{
	uint8 index;

	PS2_ClearData();
	PS2_ReadData();

	Handkey=(Data[4]<<8)|Data[3];     //这是16个按键  按下为0， 未按下为1
	for(index=0;index<16;index++)
	{	    
		if((Handkey&(1<<(MASK[index]-1)))==0)
		return index+1;
	}
	return 0;          //没有任何按键按下
}

//得到一个摇杆的模拟量	 范围0~255
uint8 PS2_AnologData(uint8 button)
{
	return Data[button];
}

//清除数据缓冲区
void PS2_ClearData()
{
	uint8 a;
	for(a=0;a<9;a++)
		Data[a]=0x00;
}
/******************************************************
Function:    void PS2_Vibration(uint8 motor1, uint8 motor2)
Description: 手柄震动函数，
Calls:		 void PS2_Cmd(uint8 CMD);
Input: motor1:右侧小震动电机 0x00关，其他开
	   motor2:左侧大震动电机 0x40~0xFF 电机开，值越大 震动越大
******************************************************/
void PS2_Vibration(uint8 motor1, uint8 motor2)
{
	CS_L;
	systick_delay_us(16);
    PS2_Cmd(0x01);  //开始命令
	PS2_Cmd(0x42);  //请求数据
	PS2_Cmd(0X00);
	PS2_Cmd(motor1);
	PS2_Cmd(motor2);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	systick_delay_us(16);
}
//short poll
void PS2_ShortPoll(void)
{
	CS_L;
	systick_delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x42);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0x00);
	CS_H;
	systick_delay_us(16);
}
//进入配置
void PS2_EnterConfing(void)
{
    CS_L;
	systick_delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x43);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x01);
	PS2_Cmd(0x00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	systick_delay_us(16);
}
//发送模式设置
void PS2_TurnOnAnalogMode(void)
{
	CS_L;
	PS2_Cmd(0x01);  
	PS2_Cmd(0x44);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x01); //analog=0x01;digital=0x00  软件设置发送模式
	PS2_Cmd(0xEE); //Ox03锁存设置，即不可通过按键“MODE”设置模式。
				   //0xEE不锁存软件设置，可通过按键“MODE”设置模式。
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	PS2_Cmd(0X00);
	CS_H;
	systick_delay_us(16);
}
//振动设置
void PS2_VibrationMode(void)
{
	CS_L;
	systick_delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x4D);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0X01);
	CS_H;
	systick_delay_us(16);
}
//完成并保存配置
void PS2_ExitConfing(void)
{
    CS_L;
	systick_delay_us(16);
	PS2_Cmd(0x01);  
	PS2_Cmd(0x43);  
	PS2_Cmd(0X00);
	PS2_Cmd(0x00);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	PS2_Cmd(0x5A);
	CS_H;
	systick_delay_us(16);
}


//MCU底层初始化
void PS2_MSP_Init()
{
	__PS2_CLK_PIN_INIT;
	__PS2_CS_PIN_INIT;
	__PS2_DO_PIN_INIT;
	__PS2_DI_PIN_INIT;
	
		//适用于STM32F103C8T6的MSPInit代码，这里用不到，所以注释了
//	GPIO_InitTypeDef GPIO_Initure;
//	__HAL_RCC_GPIOA_CLK_ENABLE();           	//开启GPIOA时钟
//	__HAL_RCC_GPIOB_CLK_ENABLE();           	//开启GPIOB时钟
//	
//	GPIO_Initure.Pin=GPIO_PIN_15; 				//PA15
//    GPIO_Initure.Mode=GPIO_MODE_OUTPUT_PP;  	//推挽输出
//    GPIO_Initure.Pull=GPIO_PULLUP;          	//上拉
//    GPIO_Initure.Speed=GPIO_SPEED_FREQ_HIGH;    //高速
//    HAL_GPIO_Init(GPIOA,&GPIO_Initure);			//PA15上拉推挽输出
//	
//	GPIO_Initure.Pin=GPIO_PIN_3|GPIO_PIN_4; 	//PB3、PB4
//	HAL_GPIO_Init(GPIOB,&GPIO_Initure);			//PB3、PB4上拉推挽输出
//	
//	GPIO_Initure.Pin=GPIO_PIN_5; 				//PB5
//    GPIO_Initure.Mode=GPIO_MODE_INPUT;  	    //输入
//    GPIO_Initure.Pull=GPIO_PULLDOWN;          	//下拉
//    GPIO_Initure.Speed=GPIO_SPEED_FREQ_HIGH;    //高速
//    HAL_GPIO_Init(GPIOB,&GPIO_Initure);			//PB5下拉输入
	
}


//手柄配置初始化
void PS2_SetInit(void)
{
	PS2_ShortPoll();
	PS2_ShortPoll();
	PS2_ShortPoll();
	PS2_EnterConfing();		//进入配置模式
	PS2_TurnOnAnalogMode();	//“红绿灯”配置模式，并选择是否保存
	PS2_VibrationMode();	//开启震动模式
	PS2_ExitConfing();		//完成并保存配置
}


void PS2_Motor_Control()
{
		uint8 key;int speed,speed1,speed2;
		if( !PS2_RedLight())
		{ 
				 systick_delay_ms(50);	 //延时很重要不可去				 
				 key = PS2_DataKey();	 //手柄按键捕获处理			
				 speed = -(PS2_AnologData(PSS_LY)-127) * 20;	 //-2550~2550  

				 if(speed >= 0) //向前
				 {
							Motor_L2_speed(speed,0);
					    Motor_R2_speed(speed,0);
				 }
				 else if(speed < 0)//向后
				 {
						 Motor_L2_speed(0,-speed);
					   Motor_R2_speed(0,-speed);
				 }
				 OLED_Print_Num1(60,1,speed);
				 OLED_Print_Num1(60,3,speed1);
				 OLED_Print_Num1(60,5,speed2);
				 if(key)
				 {
						 switch(key)
						 {
								 //机械臂一级关节
								 case PSB_L1:
								 {
										 systick_delay_ms(20);
										 if(key == PSB_L1)
										 {
											 gpio_init(D15,GPO,0); 
											 gpio_init(A17,GPO,0);
										 }
										 break;
								 }
								 case PSB_L2:
								 {
										 gpio_init(D15,GPO,1);
									   gpio_init(A17,GPO,1);
										 break;
								 }			
						 }	
				
				}
					 
	  }
		else
		{


		}
			
}
	
	













